/*
 * autonomousSystem.cpp
 *
 *  Created on: Jan 6, 2011
 *      Author: Robotics
 */
#include "AutonomousSystem.h"
#include "Encoder.h"
#include "Jaguar.h"
#include "LineSensorArray.h"
#include "ManipulatorSystem.h"
#include "MultiPIDController.h"
#include "math.h"

#define WAYPOINTSFILE "/tmp/waypointsmidright.txt"

AutonomousSystem::AutonomousSystem(DriveSystem* ptr_Drive, ManipulatorSystem* ptr_Manipulator):
	m_lineSensorArray(),
	m_PIDLeft(1.0f/15.0f,0,0,50,ptr_Drive->getGearboxPtr(false)->getEncoderPtr(), ptr_Drive->getGearboxPtr(false)),
	m_PIDRight(1.0f/15.0f,0,0,50,ptr_Drive->getGearboxPtr(true)->getEncoderPtr(), ptr_Drive->getGearboxPtr(true))
{
	m_PIDLeft.addPID(1.0f/3.0f, 0, 0, 50, (PIDSource*)&m_lineSensorArray);
	m_PIDRight.addPID(-1.0f/3.0f, 0, 0, 50, (PIDSource*)&m_lineSensorArray);
	m_PIDLeft.SetOutputRange(-0.8, 0.8,0);
	m_PIDRight.SetOutputRange(-0.8, 0.8,0);
	mptr_driveSystem = ptr_Drive;
	mptr_manipulatorSystem = ptr_Manipulator;
}

AutonomousSystem::~AutonomousSystem()
{

}

//TODO: test to make sure that it can accurataly follow line && hit desired distance (ave) && return true
bool AutonomousSystem::followLine(float distance,bool prefersRight)
{
	m_lineSensorArray.setPrefersRight(prefersRight);
	//drive along the line
	//if a "y" in the line is reached, choose a path and turn
	//once at the end, line up with the pegs
	m_PIDLeft.SetSetpoint(distance,0);
	m_PIDRight.SetSetpoint(distance,0);
	m_PIDLeft.SetSetpoint(0,1);
	m_PIDRight.SetSetpoint(0,1);
	m_PIDLeft.Enable();
	m_PIDRight.Enable();
	//printf("LineSensorPosition %i \n", m_lineSensorArray.getPosition());
	//printf("left Encoder: %f , Right Encoder: %f\n", mptr_driveSystem->getEncoderValue(false),mptr_driveSystem->getEncoderValue(false));
	return fabs(mptr_driveSystem->getAverageLREncoderValue()-distance)<3.0 && m_lineSensorArray.getPosition() == 0;
	/*while (!m_PIDLeft.OnTarget() && !m_PIDRight.OnTarget()){}
	

	//bring arm up 
	mptr_manipulatorSystem->setArmJoints(200,90);
	
	//and place the uber tube on the highest peg
	mptr_manipulatorSystem->setArmJoints(200,130);
	
	//let go
	mptr_manipulatorSystem->setClamped(false);

	//turn around,
	
	m_PIDLeft.SetSetpoint(47,0);
	m_PIDRight.SetSetpoint(-47,0);
	
	//start moving back across the field
	*/
	
}

//TODO: Test raiseArm for accuracy with given Angle
bool AutonomousSystem::raiseArm(float J1, float J2){

	return mptr_manipulatorSystem->setArmJoints(J1,J2);
	

}


/*int Autonomoussystem::lineSensorLeft()
{
	
}*/


/* Place this in driver control loop to manually push the robot into positions and record them
 * Use the trigger to record a waypoint use button 2 when finished 
 * The waypoints will be recorded into the file listed in the #define WAYPOINTFILE at the top of this file
 * 
 */
void AutonomousSystem::recordValues(KHSJoystick* ptr_joystick){
	//file open
	FILE* ptr_file = fopen(WAYPOINTSFILE,"w");
	if (ptr_file){
		printf("=======YOU ARE IN AUTONOMOUS RECORDING MODE.=======\n");
		printf("Move the bot by hand to a position and press the trigger to record.\n");
		printf("Press Button 2 to exit.\n");
		int i = 1;
		while (!ptr_joystick->GetRawButton(2)){
			//TODO: run the last arm joint using joystick y axis.
			mptr_manipulatorSystem->DriverControlJoystick(ptr_joystick);
			if(ptr_joystick->GetDebouncedButton(1)){
				//TODO: test write values to file
				printf("measurement %i taken\n",i);
				fprintf(ptr_file,"====================================%i==================================\n",i);
				fprintf(ptr_file,"Left Encoder: %f\n",mptr_driveSystem->getEncoderValue(true));
				fprintf(ptr_file,"Right Encoder: %f\n",mptr_driveSystem->getEncoderValue(false));
				fprintf(ptr_file,"Left-Right Encoder Average: %f\n",mptr_driveSystem->getAverageLREncoderValue());
				fprintf(ptr_file,"Joint 1 Angle: %f\n",mptr_manipulatorSystem->Joint1PotToAngle());
				fprintf(ptr_file,"Joint 2 Angle: %f\n",mptr_manipulatorSystem->Joint2PotToAngle());
				fprintf(ptr_file,"=======================================================================\n");
				i++;
			}
		}
		
		//file close
		fclose(ptr_file);
		printf("=======YOU HAVE LEFT AUTONOMOUS RECORDING MODE.=======\n");
	}
}

void AutonomousSystem::disable(){
	m_PIDLeft.Disable();
	m_PIDRight.Disable();
}
